#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <BLE2902.h>
#include <Wire.h>
#include <Ticker.h>

#define SERVICE_UUID         "080de97c-83f9-b6dd-3e78-2636a4b9c6d6"
#define CHARACTERISTIC_UUID  "fca38af3-b7bf-cc2f-74ff-c243f49191f7"

const char PIN_LED=12;
const char PIN_SDA=13;
const char PIN_SCL=21;
Ticker ticker;
byte DSP_I2C = (0x64U);

uint8_t writeBuff(uint8_t *buff, uint8_t size){
  uint8_t i=0;
  Wire.beginTransmission(DSP_I2C);
  for (i = 0; i < size; i++)Wire.write(buff[i]);
  return Wire.endTransmission();
}

uint8_t readBuff(uint8_t *buff, uint8_t size){
  uint8_t i=0;
  Wire.requestFrom(DSP_I2C, size);
  if (Wire.available() == size) {
    for (i = 0; i < size; i++)buff[i] = Wire.read();
    return 0;
  }
  return 1;
}

void tickerCallback(){
  digitalWrite(PIN_LED,!digitalRead(PIN_LED));
}

void blinkLed(){
  ticker.attach_ms(200,tickerCallback);
}

void lightLed(){
  ticker.detach();
  digitalWrite(PIN_LED,HIGH);
}

class ServerCallbacks : public BLEServerCallbacks{
  void onConnect(BLEServer *pServer){
    lightLed();
  };
  void onDisconnect(BLEServer *pServer){
    blinkLed();
    pServer->startAdvertising();
  }
};

class CharacteristicCallbacks : public BLECharacteristicCallbacks{
  void append(uint8_t *dest,uint8_t start,uint8_t *source,uint8_t size){
    uint8_t i=0;
    for(i=0;i<size;i++){
      dest[start+i]=source[i];
    }
  }
  void onWrite(BLECharacteristic *pCharacteristic)  {
    uint8_t *data = pCharacteristic->getData();
    uint8_t len = pCharacteristic->getLength();
    uint8_t index=data[0],cmd=data[1],*buff=data+2;
    //read
    if(cmd==0){
      uint8_t size=buff[0];
      uint8_t rst[size]={0};
      uint8_t r=readBuff(rst,size);
      if(r==0){
        uint8_t value[size+2]={index,r};
        append(value,2,rst,size);
        pCharacteristic->setValue(value,size+2);
      }else{
        uint8_t value[]={index,r};
        pCharacteristic->setValue(value,2);
      }
    }
    //write
    if(cmd==1){
      uint8_t r=writeBuff(buff,len-2);
      uint8_t value[]={index,r};
      pCharacteristic->setValue(value,2);
    }
    //notify
    pCharacteristic->notify();
  }
};

void setup(){
  Wire.begin(PIN_SDA,PIN_SCL);
  pinMode(PIN_LED,OUTPUT);
  blinkLed();
  //
  BLEDevice::init("TEF6686-BLE-Server");
  BLEServer * pServer = BLEDevice::createServer();
  pServer->setCallbacks(new ServerCallbacks());
  BLEService *pService = pServer->createService(SERVICE_UUID);

  BLECharacteristic *pCharacteristic = pService->createCharacteristic(CHARACTERISTIC_UUID,BLECharacteristic::PROPERTY_WRITE|BLECharacteristic::PROPERTY_NOTIFY);
  pCharacteristic->addDescriptor(new BLE2902());
  pCharacteristic->setCallbacks(new CharacteristicCallbacks());
  
  pService->start();
  pServer->getAdvertising()->start();
}

void loop(){
  delay(500); 
}